#include "cxcore.h"
#include "cv.h"
#include "highgui.h"

#include "display.hh"

CvFont font1;
int init_font=0;

void print_model_info(boost::shared_ptr<SMATModel>& model)
{


    std::cout << "Depth: " << model->m_depth<<std::endl;
    
    if(CV_IS_MAT(model->shape->model.mean)){
	std::cout << "Mean: "<<std::endl;
	print_arr(model->shape->model.mean);
	
	std::cout << model->shape->model.vectors->rows << "  eigenvectors."<<std::endl;
	std::cout << "Eigenvalues."<<std::endl;
	print_arr(model->shape->model.eigvals);
    }
    else{
	std::cout << "No Point Distribution Model defined"<<std::endl;
    }    

    std::cout << "Submodels: " <<model->submodel_parents.size() << std::endl;

    for(uint ai=0; ai< model->submodel_parents.size() ; ai++){
	std::cout << "Parent: " << model->submodel_parents.at(ai) << std::endl;

	print_model_info(model->submodels.at(ai));

    }

    return;
}


void print_model_state(boost::shared_ptr<SMATModel>& model)
{

    
    std::cout << "---------------" << std::endl
	      << "Depth: " << model->m_depth<<std::endl;
    
    if(CV_IS_MAT(model->shape->model.mean)){
	//some info about the shape:
	std::cout << "Shape parameter values. Base coefficients/eigenvals (first 5): " <<std::endl;
	CvMat submat;
	boost::shared_ptr<CvMat> mat (cvCreateMat(model->shape->model.vectors->rows, 2, 
						  model->shape->model.vectors->type), 
				      deleteCvMat);
	boost::shared_ptr<CvMat> st_mat(cvCreateMat(4,1,model->shape->model.vectors->type), 
					deleteCvMat);
	
	cvGetCol(mat.get(), &submat, 0);
	model->shape->get_coefficients(&submat, st_mat.get());
	cvGetCol(mat.get(), &submat, 1);
	cvCopy(model->shape->model.eigvals, &submat);
	cvGetRows(mat.get(), &submat, 0,MIN(mat->rows, 5));
	print_arr( &submat );
	
	std::cout << "Similarity Transform coeffiecients: ";
	print_arr( st_mat.get(), true);
	
    }

    for(uint ai=0; ai<model->elements.size(); ai++){
	unsigned int total = 0;
	std::cout << "Element " << ai << ": " 
		  << model->elements.at(ai)->clusters.size() << " clusters. "
		  << "Last updated: " <<  model->elements.at(ai)->m_updated_cluster
		  << std::endl;
	for(uint bi=0; bi<model->elements.at(ai)->clusters.size(); bi++){	    
	    std::cout << "\tCluster " << bi << ": " 
		      << model->elements.at(ai)->clusters.at(bi)->exemplars.size()
		      << " patches. Weight: " 
		      << model->elements.at(ai)->cluster_weight.at(bi)
		      << " Total patches: "
		      << model->elements.at(ai)->clusters.at(bi)->m_total_patches
		      << std::endl;	    

	    total += model->elements.at(ai)->clusters.at(bi)->exemplars.size();
	}
	std::cout << "Total patches: " 
		  << model->elements.at(ai)->patches.size() 
		  << " (" << total << ")" << std::endl;
    } 

    std::cout << "Submodels: " <<model->submodel_parents.size() << std::endl;

    for(uint ai=0; ai< model->submodel_parents.size() ; ai++){
	std::cout << "Parent: " << model->submodel_parents.at(ai) << std::endl;

	print_model_info(model->submodels.at(ai));

    }

    std::cout << "---------------" << std::endl;

    return;
}



void draw_model(IplImage *img, boost::shared_ptr<SMATModel>& model, CvPoint origin)
{

    boost::shared_ptr<CvMat> sh = model->shape->get_shape();
    char buffer[100];
//     CvFont font;
//     cvInitFont( &font, CV_FONT_HERSHEY_PLAIN, 0.5, 0.5);

    CvScalar color;
    if(model->m_depth == 0)
	color = DEPTH_0_COLOR;
    else if(model->m_depth == 1)
	color = DEPTH_1_COLOR;
    else
	color = DEPTH_COLOR;

    for(uint ai=0; ai<model->elements.size(); ai++){
	CvPoint p = cvPoint((int)cvmGet(sh.get(), 0, ai), (int)cvmGet(sh.get(), 1, ai));
	CvSize sz = cvSize(model->elements.at(ai)->m_size.width, 
			   model->elements.at(ai)->m_size.height);
	cvRectangle(img, p, cvPoint(p.x+sz.width, p.y+sz.height), color);
	sprintf(buffer, "%u", ai);
	cvPutText(img, buffer, cvPoint(p.x+1, p.y+sz.height-6 ), &font1, CV_RGB(200,100,0));
	
// 	char num[4];
// 	sprintf(num, "%d", ai);
// 	p.x += 2;
// 	p.y += 2;
// 	cvPutText( img, num, p , &font, color );
    }

    for(uint ai=0; ai<model->submodel_parents.size(); ai++){
	draw_model(img, model->submodels.at(ai), cvPoint((int)cvmGet(sh.get(), 0, ai), (int)cvmGet(sh.get(), 1, ai)));
    }
    
    
    return;

}

void draw_model_elements(IplImage *img, boost::shared_ptr<SMATModel>& model, 
			 CvPoint origin, float scale)
{

    unsigned int shift = 0, bi;
    char buffer[100];

    for(uint ai=0; ai<model->elements.size(); ai++){
	CvPoint pb = cvPoint(origin.x, origin.y+shift);
	CvSize sz = model->elements.at(ai)->m_size;

	if((pb.y + sz.height) >= img->height)
	    return;

	for(bi=0; bi<model->elements.at(ai)->clusters.size(); bi++){
	    unsigned int rep = model->elements.at(ai)->clusters.at(bi)->m_rep;

	    if(pb.x+(bi+1)*sz.width >= (unsigned int)img->width)
		break;

	    cvSetImageROI(img, cvRect(pb.x+bi*sz.width, pb.y, sz.width, sz.height));

	    if(img->nChannels!=1)
		cvCvtColor(model->elements.at(ai)->clusters.at(bi)->exemplars.at(rep).get(), img, CV_GRAY2BGR);
	    else
		cvCopy(model->elements.at(ai)->clusters.at(bi)->exemplars.at(rep).get(), img);

	    if(bi == model->elements.at(ai)->m_updated_cluster){
		cvRectangle(img, cvPoint(0,0), cvPoint(4,4), CV_RGB(255,0,0), CV_FILLED);
	    }

	    cvResetImageROI(img);	    
	}

	shift += sz.height;

	//Pedro 19-09-2008
	sprintf(buffer, "%u", ai);
	cvPutText (img, buffer, cvPoint(pb.x+bi*sz.width, shift), &font1, CV_RGB(255,255,255)); 
    }


    for(uint ai=0; ai<model->submodel_parents.size(); ai++){
	draw_model_elements(img, model->submodels.at(ai), cvPoint(origin.x, origin.y+shift),
			    scale);
    }

}


/*boost::shared_ptr<IplImage>*/
void display_SMATModel(IplImage * display, const IplImage *img_smat, boost::shared_ptr<SMATModel>& model)
{
    double hScale=0.40, vScale = 0.40;
    int lineWidth = 1;
    if (!init_font)
    {
	cvInitFont(&font1, CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, hScale,vScale,0,lineWidth);
	init_font=1;
    }

    IplImage  *img;

    //1.- Copy and draw on the input image.
    cvSetImageROI(display, cvRect(0,0,img_smat->width, img_smat->height));
    cvConvertImage(img_smat, display);
    
    draw_model(display, model, cvPoint(0,0));

    cvResetImageROI(display);

    //3.- Draw the elements and the representative of the patch_clusters

    img = cvCreateImage(PCLUSTER_DISPLAY_SIZE, IPL_DEPTH_32F, 3);
    IplImage *img2 = cvCreateImage(PCLUSTER_DISPLAY_SIZE, IPL_DEPTH_8U, 3);

    //check the sizes of the elements on the first level, to get an
    //adecuate scale for the elements, so the window fits
    //DISPLAY_MIN_PCLUSTERS patch clusters per element (across), and at
    //least DISPLAY_MIN_ELEMENTS elements(down)
    
    CvSize max = cvSize(-1,-1);
    for(uint ai=0; ai<model->elements.size(); ai++){
	CvSize v = model->elements.at(ai)->m_size;

	if(v.width > max.width) max.width = v.width;
	if(v.height > max.height) max.height = v.height;
    }

    draw_model_elements(img, model, cvPoint(0,0), 1);
    cvConvert(img, img2);

    CvRect r = cvRect(0,0,IMAGE_DISPLAY_SIZE.width, IMAGE_DISPLAY_SIZE.height);
    cvSetImageROI(display, cvRect(r.width,0, DISPLAY_SIZE.width-r.width, 
				  DISPLAY_SIZE.height)); 


    cvCopy(img2, display);

    cvResetImageROI(display);
    cvReleaseImage(&img);
    cvReleaseImage(&img2);

    return;
}
